/*
 * Programa para control de Arduino via Android y IOS v3.0.0
 * Autor: Jose Cruz (C) 2013
 *
 * Porciones del programa creadas por: 
 * Copyright 2012 Rackbot Modular Robotics, www.rackbot.com
 *
 * Authors: Damiano Vitulli, Dario Vitulli
 *
 * iDK Device Kit Firmware v. 1.0.0
 * iDK Full Program: Manage iDK Device Kit
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

//
//  This program requires WiFly library (Arduino 1.0 compatible).
//  Download and install the WiFly library for Arduino 1.0 in your Arduino SDK
//  You can find a version of this library here: https://github.com/jmr13031/WiFly-Shield
// 
// Para que funcione con Arduino Mega se necesita puentear
// WiFly shield ------> Mega
//
// D10 --------------> 53
// D11 --------------> 51
// D12 ---------------> 50
// D13 ---------------> 52
//

#include "WiFly.h"             // WiFly Library Functions, see above
#include "Credentials.h"       // wifi connection parameters
#include "Servo.h"             // Servo Library
#include <SoftwareSerial.h>    // Serial Communication Port Library

#include <AFMotor.h>

AF_DCMotor motor(4);
AF_DCMotor motor2(3);

//****************************************
//* Variables and Instances Initialization
//****************************************
#define iDK_VERSION "v3.0.2"            // Version de este programa

//Server Variables and Instances
Server wiflyServer(80);               // Server object initlialization on port 80
boolean wiflyClientConnected = false; // Flag to check if there is a client connected

//Commands Parser Variables
boolean cmdFlag = false;              // Flag to enable command parsing 
String cmdString;                     // Command String
String cmdRead;                       // Read String

//Se utiliza para borrar alguna linea en el GLCD
#define LINEA_BLANCO "                     "
// Se utiliza este pin del Arduino Mega para el manejo del GLCD
#define LCD_PIN 23            

//Define el objeto para el envio de comandos al GLCD
SoftwareSerial lcdSerial(0, LCD_PIN); // Assign serial lcd pins (tx, rx), since the LCD does not send data back to the Arduino, we should only define the txPin

//Servo Variables and Instances
Servo myServo;

//************************
//* Arduino Setup Function
//************************
void setup() {
  //Inicializa el GLCD
  lcdSetup();
  //Muestra la versión del programa
  lcdBacklightON(); // Switch On LCD Backlight
  lcdClear(); 
  lcdGotoLine(4); 
  lcdSerial.print("iDKSerialGLCD "); 
  lcdSerial.print(iDK_VERSION);
  delay(2000);
  //WiFly Setup and Connection
  lcdClear(); // Clear the LCD
  lcdGotoLine(1); 
  lcdSerial.print("Init WiFly..."); // Finally let's use the LCD, we display the Wifly connection information
  delay(3000); // Delay to be sure that Wifly is correctly powered on
  WiFly.begin(); // WiFly initialization 
  lcdGotoLine(2);
  lcdSerial.print("Connecting..."); // Write on LCD
  if (!WiFly.join(ssid, passphrase, false)) // WiFly connection to wifi router (ssid) using passphrase false=WEP
  { 
    // Cannot connect!
    lcdGotoLine(2);
    lcdSerial.print("Cannot connect!");
  }
  else 
  {
    // Connect successfull!
    lcdGotoLine(2);
    lcdSerial.print("Connected to LAN!");
    lcdGotoLine(3); // This time we set LCD Cursor position on second row
    lcdSerial.print("IP:");
    lcdSerial.print(WiFly.ip()); // Write the IP Address assigned from the router to the wifly board
    lcdGotoLine(4);
    lcdSerial.print("=====================");
    wiflyServer.begin(); // Start TCP/IP Server Listening
  }

  //Now we are ready to get commands, let's clear the parsing variables  
  cmdFlag = false;
  cmdString = String("");
  cmdRead = String("");
}


//***********************
//* Arduino Loop Function
//***********************
void loop() {
  Client wiflyClient = wiflyServer.available(); // Get the client connected, if any

  if (wiflyClient) 
  { // Client connected!
    if (!wiflyClientConnected) // If the client was not already connected
    {
      lcdBacklightON();
      wiflyClientConnected = true; // Set the connection flag to true 
      lcdGotoLine(5);
      lcdSerial.print("Client connected     ");
    }

    while (wiflyClient.connected()) // Repeat while client is connected
    {
      if (wiflyClient.available()) // If client is available
      {
        char c = wiflyClient.read(); //Get a byte from the client, if any
        switch (c) // Check the input byte
        {
        case '#': // Start of Command byte, this means that we are receiving a command. Command starts with a "#" character.
          cmdRead = "";
          cmdString = "";
          cmdFlag = true; // Set the command flag to true, from now on we can get the comman
          break;
        case '\r': // End of command, this means that command is fully received and we can parse it.
          if(cmdFlag == true) // If Command Flag is true
          {
            cmdString = cmdRead; // Let's save the full command 
            cmdParser(wiflyClient); // Parse it
            cmdRead = ""; // We are now ready to acquire another command
            cmdFlag = 0; // Put the command flag to false
          }
          break;
        default:
          if(cmdFlag == true) // If Command Flag is true
              cmdRead = cmdRead + String(c); // Let's store any character caming from the WifFly Port
          break;
        }
      }
    }
    delay(100); // give time to receive the data
    wiflyClient.stop(); // Stop the Client
  }
  else
  {  // Client not connected
    if (wiflyClientConnected) // If client was connected
    {
      wiflyClientConnected = false; // Set the connection flag to false
      lcdGotoLine(5);
      lcdSerial.print("Client not connected ");
      lcdGotoLine(6);
      lcdSerial.print(LINEA_BLANCO);
      lcdGotoLine(7);
      lcdSerial.print(LINEA_BLANCO);
      lcdGotoLine(8);
      lcdSerial.print(LINEA_BLANCO);
      lcdBacklightOFF();
    }
  }
}


//****************************************
// Command Parsing Function
//
// Protocolo utilizado:
//
// Conectar
// #CN0\r
//
// Activar puertos digitales
// #DOppv\r
//    pp=02 (puerto)
//    v=1 o 0 (valor)
//
// Lectura de puertos digitales
// #DI\r
// 
// Lectura puertos analogicos
// #AI\r
//
// Envio de valores PW
// #PWppv\r
//    pp (puerto)
//    v >=0 a <=9 (valor)
//
//***************************************
void cmdParser(Client wiflyClient)
{
  String cmdType; // Substring to parse Command
  String cmdPin; // Substring to parse Pin
  String cmdValue; // Substring to parse Value
  int valor=0;

  lcdGotoLine(6);
  lcdSerial.print("Comando:             ");
  lcdGoto(10,6);
  lcdSerial.print(cmdString);
  lcdGotoLine(7);
  lcdSerial.print(LINEA_BLANCO);
  lcdGotoLine(8);
  lcdSerial.print(LINEA_BLANCO);

  cmdType = cmdString.substring(0,2); // Let's get the command first two characters, they store the command type

  if(cmdType == "DO") // Digital Output Command
  {
    // Get the substring parameters
    cmdPin = cmdString.substring(2,4); // Pin
    cmdValue = cmdString.substring(4,5); // Value

    // Convert string pin to int
    char charPin[3];
    cmdPin.toCharArray(charPin,3);
    int pin = atoi(charPin); // Get the Pin number

if ((pin == 7) && (cmdValue == "1")) MotorTest1();
else
if ((pin == 7) && (cmdValue == "0")) MotorTest2();

    pinMode(pin, OUTPUT); // Set Pin Mode to Output

    if(cmdValue == "1") 
      digitalWrite(pin, HIGH); // Set the Pin Status to HIGH

      //low level
    if(cmdValue == "0"){
      digitalWrite(pin, LOW); // Set the Pin Status to LOW
    }
  }


  else if(cmdType == "DI") // Digital Input Command
  {
    lcdGotoLine(7);
    lcdSerial.print("Valores: ");

    wiflyClient.print("#DI"); // Send Ack
    // send the value of digital input pin (free pin 2 - 7)
    for (int i = 2; i < 10; i++) {
      pinMode(i, INPUT);
      int valor=digitalRead(i);
      wiflyClient.print(valor);
      lcdSerial.print(valor);
      if(i < 9){
        wiflyClient.print(",");
        lcdSerial.print(",");   
      }
    }
    wiflyClient.print("\r"); //Send end of command
  }


  else if(cmdType == "AI") // Analog Input Command
  {
    lcdGotoLine(7);
    lcdSerial.print("Valores: ");

    wiflyClient.print("#AI"); // Send Ack
    // Send the value of each analog input pin
    for (int i = 0; i < 6; i++) {
      valor=analogRead(i);
      wiflyClient.print(valor);
      lcdSerial.print(valor);
      if(i < 5){
        wiflyClient.print(",");
        lcdSerial.print(",");  
      }
    }
    wiflyClient.print("\r"); // Send end of command
  }


  else if(cmdType == "PW"){
    // Get the substring parameters
    cmdPin = cmdString.substring(2,4); // Pin
    cmdValue = cmdString.substring(4,5); // Value    

    // Convert string pin to int
    char charPin[3];
    cmdPin.toCharArray(charPin,3);
    int pin = atoi(charPin);
    // Convert string value to int
    char charValue[3];
    cmdValue.toCharArray(charValue,3);
    int value = atoi(charValue);
    myServo.attach(pin); // Set pin like servo
    
    if(value >= 0 && value <= 9)
    {
      myServo.write(value * 18); // Change servo position
      
    }
  }
}


//************************************************************************************
//* Serial Graphics LCD Utility functions
//* Modificación a la libreria de SparkFun. 
//* Realizada por Jose Cruz el 31/01/2012
//* See http://www.sparkfun.com/datasheets/LCD/SerLCD_V2_5.PDF for further information
//************************************************************************************

//Calcula el valor en pixeles para la columna. Basado en 21 car por fila de 6 pixeles cada uno. Columna entre 1 y 21 
#define lcdCol(col) (min(max(col,1),21)*6-6)
//Calcula el valor en pixeles para la fila. Basado en 8 filas de 8 pixeles cada una. Fila entre 1 y 8
#define lcdFila(fila) ((9-min(max(fila,1),8))*8-1)
//Delay para el LCD
#define lcdDelay() (delay(10))   

//Indicacion de envio de comando al LCD
#define LCD_COMANDO 0x7C
//Comandos utilizados en el Serial Graphic LCD LCD-09351 de Sparkfun
#define LCD_CLEARSCREEN 0x00
#define LCD_BACKLIGHT 0x02
#define LCD_BAUDRATE 0x07
#define LCD_COLUMNA 0x18
#define LCD_FILA 0x19

// Inicializa el LCD ajustando pines de comunicacion y colocando pantalla inicial
void lcdSetup(){
  //LCD Serial Port setup
  pinMode(LCD_PIN, OUTPUT); // Set the LCD Pin in Output mode
  lcdSerial.begin(115200); // Inicializa la libreria serial para que trabaje a 115200 Baud
  //Inicializa el baud rate en el GLCD
  lcdSerial.write(LCD_COMANDO);
  lcdSerial.write(LCD_BAUDRATE);
  lcdSerial.write("6"); // Indica al GLCD que estara trabajando a 115200
}

/* Basada en la documentación de Sparkfun */
void lcdGotoPosition(int col, int fila) 
{ 
  lcdSerial.write(LCD_COMANDO);
  lcdSerial.write(LCD_COLUMNA);
  lcdSerial.write(col);
  lcdSerial.write(LCD_COMANDO);
  lcdSerial.write(LCD_FILA);
  lcdSerial.write(fila);
  lcdDelay();
}

/*
* Nueva versión de este metodo para calcular posición en cada fila de arriba hacia abajo
 * fila 1=pixel 63, fila 2= pixel 55, fila 3= pixel 47 ....
 */
void lcdGotoLine(int fila)
{ 	
  lcdGotoPosition(0,lcdFila(fila));
}

/*
* Posiciona cursor en fila y columna
 * fila: (9-fila)*8-1 (basado en 8 pixeles de alto por caracter)
 * columna: col*6-6 (basado en 6 pixeles de ancho por caracter)
 */
void lcdGoto(int col,int fila)
{
  lcdGotoPosition(lcdCol(col),lcdFila(fila));
}

//Clear LCD
void lcdClear()
{
  lcdSerial.write(LCD_COMANDO); // Modo comando
  lcdSerial.write(byte(LCD_CLEARSCREEN)); // comando clear LCD
  lcdDelay();
}

// LCD Backlight On
void lcdBacklightON()
{  //turns on the backlight
  lcdSerial.write(LCD_COMANDO); // Command 0x7C Set the LCD Display in command mode
  lcdSerial.write(LCD_BACKLIGHT);
  lcdSerial.write(byte(50)); // Switch On backlight
  lcdDelay();
}

// LCD Backlight Off
void lcdBacklightOFF()
{  //turns off the backlight
  lcdSerial.write(LCD_COMANDO); // Command 0x7C Set the LCD Display in command mode
  lcdSerial.write(LCD_BACKLIGHT);
  lcdSerial.write(byte(0)); //Switch Off backlight
  lcdDelay();
}

//====================================================================

//int pinI1=8;//define I1 interface
//int pinI2=11;//define I2 interface 
//int speedpinA=9;//enable motor A
//int pinI3=12;//define I3 interface 
//int pinI4=13;//define I4 interface 
//int speedpinB=10;//enable motor B
//int spead =127;//define the spead of motor
//
//void forward()
//{
//     analogWrite(speedpinA,spead);//input a simulation value to set the speed
//     analogWrite(speedpinB,spead);
//     digitalWrite(pinI4,HIGH);//turn DC Motor B move clockwise
//     digitalWrite(pinI3,LOW);
//     digitalWrite(pinI2,LOW);//turn DC Motor A move anticlockwise
//     digitalWrite(pinI1,HIGH);
//}
//void backward()//
//{
//     analogWrite(speedpinA,spead);//input a simulation value to set the speed
//     analogWrite(speedpinB,spead);
//     digitalWrite(pinI4,LOW);//turn DC Motor B move anticlockwise
//     digitalWrite(pinI3,HIGH);
//     digitalWrite(pinI2,HIGH);//turn DC Motor A move clockwise
//     digitalWrite(pinI1,LOW);
//}
//void left()//
//{
//     analogWrite(speedpinA,spead);//input a simulation value to set the speed
//     analogWrite(speedpinB,spead);
//     digitalWrite(pinI4,HIGH);//turn DC Motor B move clockwise
//     digitalWrite(pinI3,LOW);
//     digitalWrite(pinI2,HIGH);//turn DC Motor A move clockwise
//     digitalWrite(pinI1,LOW);
//}
//void right()//
//{
//     analogWrite(speedpinA,spead);//input a simulation value to set the speed
//     analogWrite(speedpinB,spead);
//     digitalWrite(pinI4,LOW);//turn DC Motor B move anticlockwise
//     digitalWrite(pinI3,HIGH);
//     digitalWrite(pinI2,LOW);//turn DC Motor A move clockwise
//     digitalWrite(pinI1,HIGH);
//}
//void stop()//
//{
//     digitalWrite(speedpinA,LOW);// Unenble the pin, to stop the motor. this should be done to avid damaging the motor. 
//     digitalWrite(speedpinB,LOW);
//     delay(1000);
// 
//}
//
//void MotorTest(){
// 
// 
//
//  pinMode(pinI1,OUTPUT);
//  pinMode(pinI2,OUTPUT);
//  pinMode(speedpinA,OUTPUT);
//  pinMode(pinI3,OUTPUT);
//  pinMode(pinI4,OUTPUT);
//  pinMode(speedpinB,OUTPUT);
//
// 
//
//
//
//  left();
//  delay(2000);
//  stop();
//  right();
//  delay(2000);
//  stop();
// // delay(2000);
//  forward();
//  delay(2000);
//  stop();
//  backward();
//  delay(2000); 
//  stop(); 
//
// 
//}  



void MotorTest1() {
  uint8_t i;
  
  motor.setSpeed(200);
 
  motor.run(RELEASE);
  
  
  motor.run(FORWARD);
  for (i=0; i<255; i++) {
    motor.setSpeed(i);  
    delay(10);
 }
 
  for (i=255; i!=0; i--) {
    motor.setSpeed(i);  
    delay(10);
 }
  
  

  motor.run(BACKWARD);
  for (i=0; i<255; i++) {
    motor.setSpeed(i);  
    delay(10);
 }
 
  for (i=255; i!=0; i--) {
    motor.setSpeed(i);  
    delay(10);
 }
  

  
  motor.run(RELEASE);
  //delay(1000);
}

void MotorTest2() {
  uint8_t i;
  
  motor2.setSpeed(200);
 
  motor2.run(RELEASE);
  
  
  motor2.run(FORWARD);
  for (i=0; i<255; i++) {
    motor2.setSpeed(i);  
    delay(10);
 }
 
  for (i=255; i!=0; i--) {
    motor2.setSpeed(i);  
    delay(10);
 }
  
  

  motor2.run(BACKWARD);
  for (i=0; i<255; i++) {
    motor2.setSpeed(i);  
    delay(10);
 }
 
  for (i=255; i!=0; i--) {
    motor2.setSpeed(i);  
    delay(10);
 }
  

  
  motor2.run(RELEASE);
  //delay(1000);
}
